Vehicle control system

ABSTRACT

A vehicle control system having a controller and a spatial database adapted to provide spatial data to the controller at control speed. The spatial data provided from the spatial database to the controller can be any kind of data or information that has some relationship or association with “real world” geographical location, or if it is stored somehow with reference to geographical location. The spatial data received by the controller from the database forms at least part of the control inputs that the controller operates on to control the vehicle. The fact that the controller operates directly on information that is inherently associated with “real world” geographic location represents a change in thinking compared with existing vehicle control systems. In particular, it means that the control system of the present invention “thinks” directly in terms of spatial location. A vehicle control system in accordance with one particular embodiment of the invention comprises a task path generator, a spatial database, at least one external spatial data receiver, a vehicle attitude compensation module, a position error generator, a controller, and actuators to control the vehicle.

CROSS-REFERENCE TO RELATED APPLICATION

This application is a continuation of and claims the benefit of U.S.patent application Ser. No. 11/620,388, filed Jan. 5, 2007, entitled“Vehicle Control System,” now U.S. Pat. No. 7,835,832, issued Nov. 16,2010, which is incorporated herein by reference.

FIELD OF THE INVENTION

The present invention relates to a control system for controlling thedirection of travel of a vehicle, and in particular to a control systemhaving an embedded spatial database. The control system of the presentinvention may also be used to control other aspects of a vehicle'smotion, such as speed or acceleration. Furthermore, in the case ofagricultural vehicles and the like, the present control system may beused to control yet other aspects of the vehicle's operation, such asthe application of agricultural chemicals at desired locations(including at desired application rates), or the engagement and/or modeof operation of agricultural implements (e.g. ploughs, harvesters etc)at desired locations, etc.

For convenience, the invention will be described mainly with referenceto agricultural vehicles and like moving agricultural machinery.However, it will be clearly understood that the invention is not limitedto agricultural applications and it may equally be applied to vehiclesand other moving machinery in other areas.

BACKGROUND

A number of control systems have previously been devised for controllingthe steering of agricultural vehicles. These systems are generally usedon vehicles such as tractors (including tractors with towed tools orother implements), harvesters, headers and the like which operate inlarge fields. These vehicles generally move along predeterminedtrajectories (“paths”) throughout the field. In general, a wayline isentered into the control system and subsequent paths are calculatedbased on the wayline. If the vehicle deviates from the path as it moves,the controller causes the vehicle to steer back towards and onto thepath as described below.

As the vehicle moves along the predetermined path trajectory, it usesvarious means such as signals produced by GPS (global positioningsystem) or INS (inertial navigation system) to identify if the vehicledeviates from the desired path trajectory. If the vehicle deviates, theextent of the deviation (i.e. the difference between the actualcurvature of the vehicle's trajectory and the desired curvature, itsactual compass heading compared with the desired compass heading, andthe distance the vehicle is displaced laterally from the desired path)is expressed in the form of an error, and this error is fed back intothe control system and used to steer the vehicle back onto the desiredpath.

A problem with previous vehicle control systems is that they areinherently “one-dimensional” or “linear” in nature. This means that, ata fundamental level, the controller operates by “knowing” the path thatthe vehicle is required to traverse, and “knowing” where the vehicle islocated on that path (i.e. how far along the path the vehicle has moved)at a given time. However, the controller does not “know” where thevehicle is actually located in space. This is despite the fact that thecontroller may often progressively receive information containing thevehicle's spatial location, for example from the GPS/INS signals. Incurrent controllers, the GPS/INS signals are used primarily to determinewhen the vehicle deviates from the path (i.e. to calculate the error)rather than for the primary purpose of determining the vehicle's actualposition in space. Hence, at a fundamental level, the controller only“knows” the geometry of the path and how far the vehicle has moved alongthe path.

Therefore, with current controllers, if it is desired to know the actualspatial position of the vehicle, this must be calculated from the knowngeometry of the path and the known distance the vehicle has moved alongthat path. This calculation can be computationally expensive anddifficult to implement in practice, particularly for curved, piecewise,broken or other complex path trajectories.

By way of example, it will be appreciated that one form of common pathtrajectory that agricultural vehicles are often required to traverse infields is made up of a number of (usually parallel) path segments or“swaths” (these are sometimes also referred to as “rows”). Thus, thevehicle typically moves along one swath, harvesting or ploughing as itgoes, and it then turns around and moves back along an adjacent parallelswath, harvesting or ploughing in the opposite direction. The adjacentswath will generally be spaced from the first swath sufficiently closelythat no part of the field or crop is missed between the swaths, but alsosufficiently apart so that there is not an unnecessary overlap region(i.e. a region between the swaths that gets ploughed or harvested onboth passes). In general, the distance between the mid-lines of eachrespective swath is determined with reference to the width of thevehicle (i.e. the width of the plough, harvester or possibly the toolbeing towed by the vehicle).

In cases where paths comprising a series of parallel swaths are used,the first swath will often be used as a reference swath or “wayline”. Ingeneral, the geometry of the wayline in space will be entered into thecontrol system along with the vehicle or implement width, and this isused to calculate the required spacing (and hence trajectory) for eachof the adjacent parallel swaths. However, with most existing controlsystems, the controller is only able to control the steering of thevehicle as it proceeds along each of the swaths. It is much harder tocontrol the steering of the vehicle as it turns around between one swathand the next. Therefore, whilst the spatial geometry of the respectiveswaths may have been calculated, from the control system's point of viewat any given time it only “knows” that it is on the nth swath (numberedfrom the wayline) and that it has been moving along that swath for aknown amount of time with known speed (i.e. it knows that the vehicle isa certain distance along the nth swath). However, at a fundamentallevel, the control system does not inherently know where the vehicle isconsequently located in space or the spatial relationship between eachswath. A graphical representation of the difference between thevehicle's actual spatial location and what the control system “sees” isgiven in FIG. 1.

The “one-dimensional” or “linear” nature of existing control systemsalso causes other difficulties. One example is in relation to obstacleavoidance. In most agricultural applications, the positions of obstacles(e.g. fences, trees, immovable rocks, creeks etc) are known according totheir “real-world” spatial location. The spatial location may be knownaccording to global latitude and longitude coordinates (e.g. as providedby GPS), or alternatively the location may be known relative to a fixedpoint of known location (this is generally a point in or near the fieldused to define the origin of a coordinate system for the field).However, as current control systems only recognise where the vehicle islocated along the path, not where the vehicle is actually located inspace, the control system itself is therefore unable to recognisewhether the location of the obstacle coincides with the trajectory ofthe path, and hence whether there may be a collision.

Consequently, with current control systems, it may be necessary for anumber of separate modules to be provided, in addition to the primarycontrol module, if automatic obstacle avoidance (i.e. obstacle avoidancewithout the need for intervention by the driver of the vehicle) is to beachieved. In these cases, one of the modules would be a collisiondetection module for calculating the geometry and trajectory of asection of the path a short distance ahead of the vehicle in terms of“real world” spatial coordinates and for determining whether any of thepoints along that section of path will coincide with the location of anobstacle. If the collision detection module identifies that the sectionof path is likely to pass through an obstacle (meaning that there wouldbe a collision if the vehicle continued along that path), then a furthermodule may be required to determine an alternative trajectory for (atleast) the section of the path proximate the obstacle. Yet a furthermodule may then be required to determine how best to steer the vehiclefrom the alternative trajectory back onto the original path after thevehicle has moved past the obstacle. This multi-modular control systemstructure is complicated and can lead to computational inefficienciesbecause the different modules may each perform many of the samegeometric calculations for their own respective purposes, separatelyfrom one another, leading to “doubling up” and unnecessary computation.Also, with this modular control system structure, control of the vehiclegenerally passes from one module to another as described above, butdetermining when one module should take over from another createssignificant difficulties in terms of both system implementation andmaintenance

Another problem associated with the “one-dimensional” nature of existingcontrol systems is their inherent inflexibility and unadaptability. Forexample, in practice, if the vehicle deviates from the desired path forsome reason, it may be preferable for subsequent paths (swaths) to alsoinclude a similarly shaped deviation so that the paths remainsubstantially parallel along their length (or tangentially parallel andconsistently spaced in the case of curved sections of path). If thevehicle is, for example, a harvester or a plough, then keeping the pathsparallel in this way may help to prevent portions of the field frombeing missed, or from being harvested/ploughed multiple times (bypassing over the same portion of field on multiple passes). Even withthe modular control system structures described above, it is oftendifficult to determine the geometry of the deviated path portion interms of “real world” coordinates, and even if this can be done, it isalso difficult to adjust subsequent path geometries to correspond to thedeviation from the predetermined path trajectory that was originallyentered.

As a further example of the inherent inflexibility and unadaptability ofcurrent “one-dimensional” control systems, it is illustrative toconsider the situation where an obstacle is located near the end of oneswath such that it would be quicker and more efficient to simply move onto an adjacent swath located nearby rather than wasting time trying togo around the obstacle to finish the first swath before moving on to theadjacent swath. Current “one-dimensional” control systems are not ableto recognise that it would be more efficient to move on. This is becausethe control system only knows where the vehicle is along its currentpath (e.g. close to the end of the swath), and if a modular controlsystems is used, that module may also recognise that it is approachingthe obstacle. The control system does not know where the vehicle isactually located in space, and therefore it cannot recognise that thebeginning of the next swath is actually located nearby—it simply doesnot know where the next swath is (or indeed where the current swath isin space). Therefore, current control systems cannot easily recognisewhen it would be better to change paths (at least without interventionfrom the vehicle's driver), as this example illustrates. Nor is thecurrent “one-dimensional” structure inherently adapted to enable thecontrol systems to automatically (i.e. autonomously without assistancefrom the driver) determine and guide the vehicle along an efficienttrajectory between swaths.

It will be clearly appreciated that any reference herein to backgroundmaterial or a prior publication is not to be understood as an admissionthat any background material, prior publication or combination thereofforms part of the common general knowledge in the field, or is otherwiseadmissible prior art, whether in Australia or any other country.

DESCRIPTION OF THE INVENTION

It is an objective of the present invention to provide a vehicle controlsystem having an embedded spatial database that may at least partiallyameliorate one or more of the above-mentioned difficulties, or which mayprovide a useful or commercial alternative to existing control systems.

Accordingly, in a first broad form the present invention resides in avehicle control system having a controller and a spatial databaseadapted to provide spatial data to the controller at control speed.

In another broad form, the invention resides in a control system forcontrolling a vehicle within a region to be traversed, the controlsystem comprising

a spatial database containing spatial data,

a controller adapted to receive spatial data from the spatial databaseat control speed,

the control system being adapted to receive spatial data from thecontroller and/or an external source,

the controller using the spatial data for controlling the vehicle.

In a further broad form, a control system is provided for steering avehicle within a region to be traversed, the control system comprising

a spatial database containing spatial data,

a controller adapted to receive spatial data from the spatial databaseat control speed,

the controller being adapted to control the steering of the vehicle,

the spatial database being adapted to receive updated spatial data fromthe controller and/or an external source,

the updated spatial data relating to the vehicle and/or an implementassociated with and proximate the vehicle and/or at least a portion ofthe region proximate the vehicle.

In agricultural applications, the region to be traversed by the vehiclewill generally be the field that is to be ploughed, harvested, etc, andthe invention will be described generally with reference to agriculturalvehicles operating in fields. However, no limitation is meant in thisregard, and the region to be traversed by the vehicle may take a rangeof other forms in different applications. For example, in automotiveapplications the region to be traversed by the vehicle might compriseroadways located in a particular geographical area. Alternatively, inmining applications the region could comprise the vehicle navigableregions of the mine. In underground mining, this could include thevarious levels of the mine located vertically above and below oneanother at different relative levels (depths). Furthermore, the controlsystem of the present invention could be applied to vehicles thatoperate on airport tarmacs, in which case the region to be traversed bythe vehicle might be the tarmac, or a portion thereof. From theseexamples, the person skilled in the art will appreciate the breadth ofother applications that are possible.

The control system of the present invention includes a spatial databasethat contains spatial data. The spatial database may also be adapted toreceive spatial data including updated spatial data, and to providespatial data to other components of the control system. In general, datamay be characterised as “spatial” if it has some relationship orassociation with “real world” geographical location, or if it is storedsomehow with reference to geographical location. Some illustrativeexamples of the kinds of spatial data that may be stored within thedatabase include (but are not limited to) coordinate points describingthe location of an object (e.g. a rock or tree) in terms of the object's“real world” geographical location in a field, the coordinate points fora geographical location itself, information regarding a “state” of thevehicle (e.g. its speed, “pose” (position and orientation) or even fuellevel) at a particular geographical location, a time when the vehiclewas at a particular geographical location, or a command to the vehicleto change its trajectory or mode of equipment (e.g. plough) operation ifor when it reaches a certain geographical location. These examplesillustrate that any data or information that has an association withgeographical location, or which is stored with reference to geographicallocation, can constitute “spatial data”. For the remainder of thisspecification, the terms “spatial data” and “spatial information” willbe used interchangeably. References simply to “data” or “information”will generally also carry a similar meaning, and references simply tothe “database” will be to the spatial database, unless the contextrequires otherwise. Typically, the spatial database is an electronicdatabase stored in a memory device, such as, for example, a RAM, asdiscussed in more detail below.

Spatial data may be stored within the database according to anyconvenient coordinate system, including (but not limited to) cartesian(or projected) coordinates, polar coordinates, cylindrical coordinates,spherical coordinates, latitude/longitude/altitude etc. The coordinatesystem may also be “global” in the sense of the location referencesprovided by GPS, or “local” coordinates such as those defined withrespect to a local origin and reference orientation. The coordinates mayor may not take into account the curvature caused by the Earth's overallspherical shape. Hence, there is no limitation as to the coordinatesystem that may be used with the present invention, although it isenvisaged that Cartesian (x,y or x,y,z) coordinates orlatitude/longitude/altitude will be used most frequently because of theway these inherently lend themselves to describing geographicallocation, and because of the ease with which these coordinate systemscan be implemented digitally. Particularly representative embodimentsmay utilise the WGS84 datum which is consistent with the current GPS.

Those skilled in the art will know that GPS (global positioning system)is the name of the satellite based navigation system originallydeveloped by the United States Department of Defense. GPS is now used ina wide range of applications. A number of systems also exist forincreasing the accuracy of the location readings obtained using GPSreceivers. Some of these systems operated by taking supplementaryreadings from additional satellites and using these supplementaryreadings to “correct” the original GPS location readings. These systemsare commonly referred to as “Satellite Based Augmentation Systems”(SBAS) and some examples of SBASs are:

The United States'“Wide Area Augmentation System” (WAAS),

The European Space Agency's “European Geostationary Navigation OverlayService” (EGNOS), and

The Japanese' “Multi-Functional Transportation Satellite” (MFTS).

A number of “Ground Based Augmentation Systems” (GBASs) also exist whichhelp to increase the accuracy of GPS location readings by takingadditional readings from beacons located at known locations on theground. It will be understood that, throughout this specification, allreferences to GPS include GPS when augmented by supplementary systemssuch as SBASs, GBASs and the like.

It is explained above that the controller (which controls the vehicle)receives spatial data from the spatial database. In this way, the datareceived by the controller from the database forms at least part of thecontrol inputs that the controller operates on to control the vehicle(i.e. the spatial data forms at least part of the inputs that drive thecontroller). The fact that the controller operates directly oninformation that is inherently associated with “real world” geographiclocation represents a change in thinking compared with existing vehiclecontrol systems. In particular, it means that the control system of thepresent invention “thinks” directly in terms of spatial location. Putanother way, in the control system of the present invention, controlparameters are defined in geographic space rather than the space of anabstract vector. Consequently, the controller of the present inventionmay be considered to be inherently “multi-dimensional” or “spatial” innature, as opposed to “one-dimensional” or “linear” like the existingcontrol systems described in the background section above.

It is envisaged that at least some (and probably most) of the componentsof the control system, including the controller, will typically beimplemented using commercially available equipment and a generallyconventional control architecture. For instance, the controller may beimplemented using equipment that provides memory and a centralprocessing unit to run the one or more algorithms required to controlthe vehicle. Likewise, the controller (and hence the controlalgorithm(s)) used in the present invention may take any form suitablefor controlling the steering of a vehicle. Typically, closed loop orfeedback type control will be used at least in relation to some signalstreams (i.e. in relation to at least some of the vehicle variablesbeing controlled by the controller). However, open loop control may alsobe used, as may feed-forward control structures wherein the spatial datareceived by the controller from the spatial database is fed forward toform part of the control outputs used to control the vehicle. Wherefeedback type control is used, the control structure may incorporatecombinations of proportional, integral and differential control, or aseries of such (possibly nested) control loops. However, no particularlimitation is meant in this regard and the person skilled in the artwill appreciate that any form of suitable control and/or controller maybe used.

The control system may also incorporate conventional signal processingand transmitting equipment, for example, for suitably filtering incomingspatial data signals, and for transmitting control signals from thecontroller to the vehicle's steering system to steer the vehicle. Theperson skilled in the art will appreciate that any suitable electric,mechanical, pneumatic or hydraulic actuators, or combinations thereof,may be used with the present invention. The actuators may be linked withthe vehicle's steering and drive systems to control the steering,acceleration, deceleration etc of the vehicle in response to controlsignals produced by the controller. Associated equipment such asamplifiers and power sources may also be provided as required to amplifythe control signals, and to power the actuators. A wide range of powersources may be used including batteries, generators, pumps etc dependingon the nature of the actuator(s) and the signals to be amplified.

Whilst the present control system may operate using a conventional formof controller and using at least some commercially available equipment,the spatial database used to store the spatial data and to provide thespatial data to the controller may be different to other forms ofdatabases used in other areas. In other areas (including non-controlrelated applications such as those where data storage is the principalobjective), databases often contain the vast amounts of information (inthis case “information” is not used in its “spatial” sense) and theinformation is generally stored in complex hierarchical structures.Conceptually, these databases may be considered to be “multi-levelled”in that an initial query may return only relatively superficial levelinformation, but this may in turn allow the user to interrogate thedatabase more deeply to obtain more specific, linked or relatedinformation. This complex structure means that these kinds of databasescan take considerable time (many seconds, minutes or even longer) togenerate the appropriate output in response to a query. Those skilled inthe art will appreciate that databases such as these, which take arelatively long time to return information in response to a query, maynot be suitable for use in control systems such as the present whichrequire low latencies between variable inputs and control outputs tothereby enable real-time control to be provided.

The spatial database used in the present invention will suitably beadapted to provide the data to the controller at control speed. In thissense, “control speed” means that the database is able to provide theinformation at a rate of the same order as the speed at which thecontroller repeats successive cycles of the control algorithm (i.e. at arate of the same order as the “clock speed” of the controller). Ideally,the database will be adapted to provide the data to the controller, andperhaps also receive data from the controller and/or external sources,at every successive cycle of the control algorithm (i.e. at thecontroller's clock speed). However, in some embodiments it may besufficient for the database to be adapted to provide (and perhapsreceive) data at less than, but close to, the controller's clock speed(for example, at every second or third successive cycle of the controlalgorithm), provided that the rate is fast enough to provide thecontroller with sufficiently up-to-date spatial information to achieveadequate vehicle control performance. In cases where the controlleroperates at different clock speeds for different data signal streams,the database may be adapted to provide data at a rate of the same orderas one of those controller clock speeds. In any event, the databaseshould provide data to the controller at a rate commensurate with thecontrol loop bandwidth.

In practice, it is envisaged that the database may be adapted to providedata to the controller at a rate of between 1 Hz and 100 Hz. Given thespeeds that vehicles such as agricultural vehicles typically move at(generally less than 60 km/hr or 37.3 miles/hr), rates between 1 Hz and20 Hz will almost always be sufficient, and even rates between 3 Hz and12 Hz may be sufficient for vehicles moving at significantly less than60 km/hr. Nevertheless, those skilled in the art will recognise that thenecessary or achievable rates may vary depending on the level of controlprecision and performance required in different applications, the speedat which the vehicle in question moves, and the capabilities of theavailable equipment used to implement the control system.

Those skilled in the art will appreciate that because the spatialdatabase used in the present invention can provide spatial data to thecontroller at control speed, and therefore forms part of the system'soverall configuration, the spatial database may be considered to be“embedded” within the control system, rather than external to it. Thisis particularly so in embodiments where feedback type control is used,and the spatial database forms part of the system's overall closed loopstructure (i.e. in embodiments where the spatial database forms part ofthe loop).

In order for the database to be able to provide (and, if desired, alsoreceive) data at the required rates, the form of the database shouldallow the required rapid database access and response times. Ideally,the database and all of the data that it contains will be loaded intothe control system's memory (i.e. loaded into RAM). This way, the datawill be directly accessible by the controller's CPU (central processingunit), rather than requiring a query to be sent to a remote disk orstorage device containing the data, the response to which would thenneed to be loaded into RAM before being accessible by the CPU. However,it is possible that the database could be located on a separate disk orother storage device, particularly if the device is capable ofretrieving data in response to a query with sufficient speed such as,for example, a disk device with RAM read/write cache.

It is envisaged that the amount of memory required to store the spatialdata relating to a particular field to be traversed by the vehicle maybe in the order of megabytes. By way of example (given for illustrativepurposes only), consider a straight wayline that is 1 km long and whichhas 500 parallel swaths of corresponding length. If the database isdesigned to incorporate information pertaining to locations every 2 malong each of the 500 swaths, this corresponds to 501.times.500=250,500locations. When the data is structured within the database in the mannerdescribed further below, this may correspond to approximately 4 MB ofmemory required to store the coordinates of each point. However, it isalso envisaged that as the nature and complexity of the data required tobe stored in the database increases, the required amount of memory mayincrease to hundreds of megabytes or gigabytes. Devices which providethis amount of memory are (or are at least becoming) commerciallyavailable.

The speed of the database may be assisted by the way in which the datais arranged (i.e. stored) within the database. A wide range of methodsand algorithms are known for arranging data (i.e. for assigningappropriate “indices” and the corresponding memory allocations toindividual items of data) within databases, and the particular methodchosen depends on the nature of the data, and the way and speed withwhich the database is to respond to a query. For the complexhierarchical “multi-levelled” databases described above, the data shouldbe arranged so as to enable the database to collate and deliver allrelevant information relating to a complex query. However, as explainedabove, the requirement for those databases to be able to process complexqueries leads to potentially long lag times which may be undesirable inthe context of vehicle control applications. Therefore, the spatialdatabase used in the present invention can store data in a“single-level” or “flat” structure according to the geographicallocation that particular items of data relate to.

Some algorithms which could be used to arrange the spatial data withinthe database include the algorithms commonly referred to by the names“Grid-indexing”, “Quadtree” or “R-tree”. However, in other embodimentsof the invention data may be arranged within the database using a formof algorithm that will be referred to as a “spatial hash-key” algorithm.A spatial hash-key algorithm maps physical locations (based on their“real world” coordinates) into one-dimensional “hash-keys”. The“hash-key” for each location is a string of characters that can bestored in the database's hash table and retrieved in response to aquery.

Properties of the spatial hash-key algorithm may include:

points which are close to each other in the real world should haveclosely related hash keys (i.e. the algorithm should maintain“locality”), the algorithm should operate using whatever coordinatesystem the control system uses to represent the region,

the algorithm should be adapted for digital implementation (hence, itshould be adapted to operate using integer or floating-point numbers,preferably with 64-bit “double” precision or better)

the algorithm should be fast to compute.

It is explained above that the control system of the present invention,and ideally the spatial database, may be adapted to receive updated datafrom the controller and/or an external source. The spatial database canbe adapted to receive the updated information at control speed. Datareceived from the controller may include or may be used to generate, forexample, estimates of the vehicle's predicted state (i.e. its speed,position, orientation etc) at an upcoming location based on its currentinstantaneous state at a particular location. The external sources mayinclude GPS, INS, or any other inertial, visual or other system used forobtaining information relating to the state of the vehicle or otheraspects of the region (such as obstacles close to the vehicle). Datareceived in this way may be (at least initially) recorded in itsunprocessed or “raw” form in the database. This unprocessed data may befed directly back into the controller, or the respective streams ofincoming data (possibly relating to disparate variables) may be filteredusing a Kalman filter or some other similar digital signal processingtechnique to obtain a statistically optimised estimate of the state ofthe vehicle and its proximate surroundings as it travels. This optimisedestimate of the vehicle's state at a particular location may then be fedinto the controller. The use of statistically optimised estimates anddata may help to improve control performance.

According to a further broad form, the invention resides in a closedloop vehicle control system comprising

a spatial database,

a controller adapted to receive spatial data from the spatial databaseat control speed, the controller controlling the steering of thevehicle,

wherein updated spatial data is fed back into the control system.

In yet another broad form, the invention resides in a method forcontrolling a vehicle comprising

entering spatial data relating to a region to be traversed by thevehicle into a spatial database,

providing spatial data from the spatial database to a controller atcontrol speed to control the vehicle as the vehicle traverses theregion, and

entering updated spatial data into the spatial database as the vehicletraverses the region.

In yet a further broad form, the invention resides in a vehicle controlsystem comprising

a spatial database,

a controller adapted to receive spatial data from the spatial database,

the controller using the spatial data from the spatial database tocontrol the steering of the vehicle.

It will be appreciated that all preferred features and aspects of theinvention described with particular reference to one or other broad formof the invention, may also apply equally to all other forms of theinvention, unless the context dictates otherwise.

BRIEF DESCRIPTION OF THE DRAWINGS

Certain embodiments, aspects and features of the invention will now bedescribed and explained by way of example and with reference to thedrawings. However, it will be clearly appreciated that thesedescriptions and examples are provided to assist in understanding theinvention only, and the invention is not limited to or by any of theembodiments, aspects or features described or exemplified.

FIG. 1 schematically represents the difference between the vehicle'sactual spatial location and what is “seen” by existing forms of“one-dimensional” controllers such as those described in the backgroundsection above,

FIG. 2 is a pictorial representation of an agricultural vehicle having acontrol system in accordance with one particular embodiment of thepresent invention,

FIG. 3 illustrates the physical meaning of certain parameters controlledby some versions of the present control system, namely the “cross-trackerror”, the “heading error” and the “curvature error”,

FIG. 4 is a schematic “block-diagram” representation of an overallcontrol system structure that may be used in representative embodimentsof the present invention,

FIG. 5 is a schematic representation of the “controller” block that maybe used in representative embodiments such as that shown in FIG. 4,

FIG. 6 is a further schematic “block-diagram” representation of anoverall control system structure that may be used with alternativerepresentative embodiments of the invention which incorporate additionalfeatures not shown in FIG. 4.

FIG. 7 is a block diagram representation of the state spacerepresentation used in the digital implementation of certain aspects ofthe control system.

FIG. 8 shows an example trajectory of an agricultural vehicle, and thecoordinates corresponding to different points along the trajectory usinga simplified integer based coordinate system.

FIG. 9 shows a similar example trajectory of an agricultural vehicle tothat shown in FIG. 8, except that the coordinate system is similar informat to the WGS84 coordinate used by current GPS.

FIG. 10 illustrates the way in which numbers are represented in the IEEE754 standard double-precision floating-point format, and

FIG. 11 is a “flow-diagram” illustrating the way a particularlypreferred spatial hash algorithm may be used to generate hash keys forthe coordinates in FIG. 9.

BEST MODE

As described in the background section above, one of the problems withexisting vehicle control systems is that they are inherently“one-dimensional” or “linear” in nature. The inherent “linear” nature ofexisting control systems is illustrated schematically in FIG. 1. Whilstthe “real world” spatial geometry of the respective swaths shown on theleft in FIG. 1 may have been calculated, nevertheless from the controlsystem's point of view at any given time the controller only “knows”that the vehicle is on the nth swath and that it has been moving alongthat swath for a known amount of time with known speed. Hence, at afundamental level, the controller does not inherently know where thevehicle is located in space. This is represented graphically in FIG. 1.

Next, FIG. 2 shows an agricultural vehicle 1 having a control system inaccordance with one embodiment of the present invention. In FIG. 2, theagricultural vehicle 1 is a tractor towing an implement 2. The implement2 could be a plough, harvester, seed sower, leveller, agriculturalchemical applicator/dispenser or any other kind of agriculturalimplement. Furthermore, the embodiment of the invention shown in FIG. 2could equally be applied on other kinds of vehicles operating in otherareas, for example cars, mine-trucks, airport tarmac vehicles, etc.

The components of the control system in the particular embodiment shownin FIG. 2 include a main control unit 3, a GPS antenna 4 and actuators5. The main control unit 3 houses the spatial database and also theelectronic hardware used to implement the controller. The main controlunit 3 may be an industrial computer (for example an industrial PC)capable of running other applications in addition to the vehicle controlsystem. Alternatively, the main control unit 3 may be a purpose-builtunit containing only the hardware required to run the controller, thespatial database and the other components of the vehicle control system.

The main control unit 3 receives GPS signals from the GPS antenna 4, andit uses these (typically in combination with feedback and/or otherexternal spatial data signals) to generate a control signal for steeringthe vehicle. The control signal will typically be made up of a number ofcomponents or streams of data relating to the different parameters ofthe vehicle being controlled, for example the vehicle's “cross-trackerror”, “heading error”, “curvature error”, etc. These parameters willbe described further below. The control signal is amplified usingsuitable signal amplifiers (not shown) to create a signal that issufficiently strong to drive the actuators 5. The actuators 5 areinterconnected with the vehicle's steering mechanism (not shown) suchthat the actuators operate to steer the vehicle as directed by thecontrol signal.

In some embodiments, further actuators (not shown) may also be providedwhich are interconnected with the vehicle's accelerator and/or brakingmechanisms, and the control signal may incorporate components or signalstreams relating to the vehicle's forward progress (i.e. its forwardspeed, acceleration, deceleration etc). In these embodiments, thecomponent(s) of the control signal relating to the vehicle's forwardprogress may also be amplified by amplifiers (not shown) sufficiently tocause the actuators which are interconnected with theaccelerator/braking mechanism to control the vehicle'sacceleration/deceleration in response to the control signal.

The vehicle 1 may also be optionally provided with one or more visualsensors 6, one or more inertial sensors 7 and a user terminal 8. Oneform of visual sensor 6 that may be used may operate by receiving imagesof the ground beneath the vehicle, preferably in rapid succession, andcorrelating the data pertaining to respective successive images toobtain information relating to the vehicle's motion. Other forms ofvisual sensor may also be used including LIDAR (Light Detection andRanging) or sensors which operate using machine vision and/or imageanalysis. If present, the one or more inertial sensors 7 will typicallyinclude at least one gyroscope (e.g. a rate gyroscope), although theinertial sensors 7 could also comprise a number of sensors andcomponents (such as accelerometers, tilt sensors and the like) whichtogether form a sophisticated inertial navigation system (INS). Thevehicle may be further provided with additional sensors (not shown) suchas sensors which receive information regarding the location of thevehicle relative to a fixed point of known location in or near thefield, magnetometers, ultrasonic range and direction finding and thelike. The data generated by these additional sensors may be fed into thedatabase and used by the control system to control the vehicle asdescribed below.

In embodiments where the main control unit 3 comprises an industrial PCor the like, the user terminal 8 may comprise a full computer keyboardand separate screen to enable the user to utilise the full functionalityof the computer. However, in embodiments where the main control unit isa purpose-built unit containing only hardware relating to the vehicle'scontrol system, the terminal 8 may comprise, for example, a singlecombined unit having a display and such controls as may be necessary forthe user to operate the vehicle's control system. Any kind of controlsknown by those skilled in this area to be suitable may be used on themain control unit, including keypads, joysticks, touch screens and thelike.

In FIG. 2, the user terminal 8 is positioned in the vehicle cabin sothat it can be operated by the driver as the vehicle moves. However,those skilled in the art will recognise that the present control systemcould also be operated by wireless remote control, meaning that the userterminal 8 could alternatively be totally separate from the vehicle andcould operate the vehicle's control system from a remote location. It isalso envisaged that a single remote user terminal 8 may be used towirelessly interface with the control systems of multiple vehicles(possibly simultaneously) so that the user can control multiple movingvehicles from the one remote terminal.

In order to control the steering of the vehicle, there are threeparameters that should be controlled. These are the “cross-track error”,the “heading error” and the “curvature error”. The physical meaning ofthese parameters can be understood with reference to FIG. 3. The“cross-track error” is the lateral difference between the vehicle'sactual position, and its desired position. This is illustrated by the“{” bracket in FIG. 3. The “heading error” is the difference between thevehicle's actual instantaneous direction of motion h (i.e. its actualcompass heading), and its desired instantaneous direction of motion H.The heading error is given by:

Heading Error=H−h

Those skilled in the art will recognise that both h and H are inherentlydirectional quantities.

Finally, the “curvature error” is the difference between the actualinstantaneous radius of curvature r of the vehicle's motion and thedesired instantaneous radius of curvature R. The curvature error isgiven by:

Curvature Error=1/R−1/r

It will also be clearly appreciated that there may be many other vehiclevariables or parameters which also need to be controlled if, forexample, acceleration/deceleration or the vehicle's mode of equipmentoperation are also to be controlled.

Referring next to FIG. 4, it can be seen that a vehicle control systemin accordance with one particular embodiment of the invention comprises:

a task path generator,

a spatial database,

at least one external spatial data source,

a vehicle attitude compensation module,

a position error generator,

a controller, and

actuators to control (steer) the vehicle.

In the overall operation of the control system, the desired pathtrajectory for the vehicle is first entered into the control system bythe user via the user terminal 8. The task path generator theninterprets this user-defined path definition and converts it into aseries of points of sufficient spatial density to adequately representthe desired path to the requisite level of precision. The task pathgenerator typically also defines the vehicle's desired trajectory alongthe user-defined path, for example, by generating a desired vehicleposition, a desired heading H and a desired instantaneous radius ofcurvature R for each point on the path. This information is then loadedinto the spatial database. The way in which this and other spatialinformation is stored within the database in representative embodiments,and in particular the way in which pieces of data are given memoryallocations according to their spatial location, is described furtherbelow.

As the vehicle moves along the user-defined path, it will invariablyexperience various perturbations in its position and orientation due to,for example, bumps, potholes, subsidence beneath the vehicle's wheels,vehicle wheel-spin, over/under-steer etc. Those skilled in this areawill recognise that a huge range of other similar factors can alsoinfluence the instantaneous position and orientation of the vehicle asit moves. One of the purposes of the present control system is toautomatically correct for these perturbations in position andorientation to maintain the vehicle on the desired path (or as close toit as possible).

As the vehicle moves, the control system progressively receives updatedinformation regarding spatial location from the external spatial datasources. The external spatial data sources will typically include GPS.However, a range of other spatial data sources may also be used inaddition to, or in substitute for GPS. For example, the inertialnavigation systems (INS), visual navigation systems etc described abovemay also be used as external data sources in the present control system.

Those skilled in the art will recognise that the spatial data collectedby the external spatial data sources actually pertains to the specificlocation of the external spatial data receivers, not necessarily thevehicle/implement reference location itself (which is what is controlledby the control system). In FIG. 2, the reference location is on thevehicle 1 and is indicated by the intersection (i.e. the origin) of theroll, pitch and yaw axes. In other embodiments, the reference locationmay be located elsewhere on the vehicle, or on the implement 2 etc. Inany event, to illustrate this point, it will be seen that the GPSantenna 4 in FIG. 2 is located on the roof of the vehicle some distancefrom the vehicle's reference point. Therefore, the spatial datacollected by the GPS antenna actually relates to the instantaneouslocation of the vehicle's roof, not the location of the vehicle'sreference point. Likewise, the spatial data collected by the visualsensor 6 actually pertains to the particular location of the visualsensor (slightly out in front of the vehicle in FIG. 2).

In addition to this, changes in the vehicle's attitude will alsoinfluence the spatial position readings received by the differentreceivers. For example, if one of the vehicle's wheels passes over, oris pushed sideways by a bump, this may cause the vehicle to rotate aboutat least one (and possibly two or three) of the axes shown in FIG. 2.This will in turn change the relative position of the spatial datareceiver(s) such as GPS antenna 4 with respect to the reference locationon the vehicle or implement. This can be used (typically in combinationwith other sources of external spatial data or “feedback” data) todetermine the orientation of the vehicle. The orientation of the vehiclemay be considered to be the relative orientation of the vehicle's axesin space.

In order to compensate for the difference in position between thevehicle's reference point and the location of the spatial datareceiver(s), and also to account for changes in the vehicle'sorientation, a vehicle attitude compensation module is provided. This isshown in FIG. 4. The vehicle attitude compensation module converts allreadings taken by the various spatial data receivers (which relate tothe different specific locations of the receivers) into readingspertaining to the spatial location and orientation of the vehicle'sreference point. This data pertaining to the spatial location andorientation of the vehicle's reference point is then fed into thespatial database.

Those skilled in the art will recognise that the one or more externalspatial data sources will progressively receive updated data readings inrapid succession (e.g. in “real time” or as close as possible to it).These readings are then converted by the vehicle attitude compensationmodule and fed into the spatial database. The readings may also befiltered as described above. Therefore, whilst each reading from eachspatial data source is received, converted (ideally filtered) andentered into the spatial database individually, nevertheless the rapidsuccessive way in which these readings (possibly from multiple“parallel” data sources) are received, converted and entered effectivelycreates a “stream” of incoming spatial data pertaining to the vehicle'scontinuously changing instantaneous location and orientation. In orderto provide sufficient bandwidth, successive readings from each externalspatial data source should be received and converted with a frequency ofthe same order as the clock speed (or at least one of the clock speeds)of the controller, typically 3 Hz-12 Hz or higher.

Referring again to FIG. 4, the position error generator next receivesinformation from the spatial database. The information it receives fromthe database includes:

the vehicle's desired position, heading H and instantaneous radius ofcurvature R. It will be recalled that this information is originallygenerated by the task path generator and then entered into the spatialdatabase, based on the user-defined path trajectory.

And the vehicle's actual position, heading h and instantaneous radius ofcurvature r.

This information is based on spatial data progressively received fromthe external spatial data sources as described above, and typically alsoon data received through feedback.

The position error generator then uses this information to calculate aninstantaneous “error term” for the vehicle. The “error term”incorporates the vehicle's instantaneous cross-track error, headingerror and curvature error (as described above). The error term is thenfed into the controller. The controller is shown in greater detail inFIG. 5.

From FIG. 5 it can be seen that the controller incorporates across-track error PID controller, a heading error PID controller and acurvature error PID controller. The PID controllers used with thepresent invention are of a conventional form that will be wellunderstood by those skilled in this area and need not be described indetail. The output from the cross-track error, heading error andcurvature error PID controllers then passes through a curvature demandsignal integrator. The output from the PID controllers is thereforeintegrated in order to generate a curvature demand signal. Thiscurvature demand signal is thus the “control signal” which is amplifiedby amplifiers (not shown) before proceeding to drive the actuators asrequired. In other words, the signal obtained by integrating the outputfrom the PID controllers is amplified and sent to the actuators in theform of a curvature demand to change the vehicle's steering angle andhence steer the vehicle back onto the desired path. Finally, the changein vehicle pose etc caused by the control driven change in steeringangle is registered via the updated information received through theexternal data sources (GPS etc) and the vehicle's new position, headingand instantaneous radius of curvature are re-entered into the spatialdatabase to complete control system's overall closed loop controlstructure. It will be noted that the arrows extending from theactuators/steering mechanism to the external data sources in FIG. 4 aredashed rather than solid lines. This is to indicate that, whilst thereis no actual control signal or other data flow from theactuators/steering mechanism to the external data sources, there isnevertheless a causal link between the change in vehicle pose etc causedby the control driven change in steering angle and the updatedinformation received through the external data sources.

In FIG. 6, there is shown a slightly more elaborate embodiment of thecontrol system.

The embodiment shown in FIG. 6 is generally the same as that shown inFIG. 4, except that the embodiment in FIG. 6 incorporates an optimisingfilter and an external obstacle detection input. The optimising filtercan operate to statistically optimise at least some of the spatial datacontained in the spatial data base. Also, the filter will generallyoperate as an “observer”, meaning that it does not form part of thecontrol loop. Rather, the filter will typically reside outside thecontrol loop and it will generally operate by taking data directly fromthe database and returning optimise data directly into the database, asshown in FIG. 6. More specifically, the filter will take the updated“feedback” data that re-enters the database from the control loop(described above) together with the updated spatial data obtained fromthe external spatial data sources (after it has been processed by thevehicle attitude compensation module) and it will then use thesedisparate streams of data to calculate a statistically optimised updatedestimate of, for example, the vehicle's instantaneous position, headingand radius of curvature. The filter will typically comprise a Kalmanfilter.

The external obstacle detection input may comprise any form of visionbased, sound based or other obstacle detection means, and the obstacledetection data may be converted by the vehicle attitude compensationmodule (just like the other sources of external data discussed above)and then fed into the spatial database. Where the control systemincorporates obstacle detection, it is then necessary for the task pathgenerator to be able to receive updated information from the spatialdatabase. This is so that if an obstacle is detected on the desiredpath, an alternative path that avoids the obstacle can be calculated bythe task path generator and re-entered into the database. The ability ofthe task path generator to also receive data from the spatial databaseis indicated by the additional arrow from the spatial database to thetask path generator in FIG. 6.

FIGS. 4-6 graphically represent the operation of the control system.However, it is also useful to consider the way in which the vehicle'sparameters and dynamics are represented for the purposes of implementingthe control system. Those skilled in the art will recognise that a rangeof methods may be used for this purpose. However, it is considered thatone method is to represent the parameters and dynamics in “state space”form.

In state space representations, the variables or parameters used tomathematically model the motion of the vehicle, or aspects of itsoperation, are referred to as “states” x_(i). In the present case, thestates may include the vehicle's position (x,y), velocity

$\left( {\frac{x}{t},\frac{y}{t}} \right)$

heading h, radius of curvature r etc. Hence the states may includex_(i)=x, x₂=y, x₃=h, x₄=h,

${x_{5} = {\frac{x}{t} = \frac{{dx}_{1}}{t}}},{x_{6} = {\frac{y}{t} = \frac{x_{2}}{t}}}$

Etc. However, it will be appreciated that the choice of states is neverunique, and the meaning and implications of this will be well understoodby those skilled in the art.

The values for the individual states at a given time are represented asthe individual entries in an n×1 “state vector”:

X(t)=[x ₁(t)x ₂(t)x ₃(t)x ₄(t) . . . x _(n)(t)]^(T)

where n is the number of states.

In general, the mathematical model used to model the vehicle's motionand aspects of its operation will comprise a series of differentialequations. The number of equations will be the same as the number ofstates. In some cases, the differential equations will be linear interms of the states, whereas in other situations the equations may benonlinear in which case they must generally be “linearised” about apoint in the “state space”. Linearisation techniques that may be used todo this will be well known to those skilled in this area.

Next, by noting that any j^(th) order linear differential equations canbe re-written equivalently as a set j first order linear differentialequations, the linear (or linearised) equations that represent the modelcan be expressed using the following “state” equation:

${\frac{}{t}\left( {\underset{\_}{X}(t)} \right)} = {{A{\underset{\_}{X}(t)}} + {B{\underset{\_}{U}(t)}} + {E{\underset{\_}{w}(t)}}}$

where:

A is an n×n matrix linking the state time derivatives to the statesthemselves,

U(t) is an m×1 matrix containing the external “forcing” inputs in themathematical model,

B is an n×m matrix linking the state derivatives to the inputs,

m is the number of inputs,

Ew(t) is a quantity (represented by an n×1 vector) called the “processnoise”.

The process noise represents errors in the model and vehicle dynamicswhich exist in the actual vehicle but which are not accounted for in themodel. As Ew(t) represents an unknown quantity, its contents are notknown. However, for reasons that will be understood by those skilled inthis area, in order to allow statistically optimised signal processingand state estimation Ew(t) is generally assumed to be Gaussian, white,have zero mean and to act directly on the state derivatives. It is alsoassumed that the process noise element associated with each individualstate is uncorrelated with the process noise element of the otherstates.

The quantities that are desired to be known about the vehicle (the realvalues for which are generally also measured from the vehicle itself, ifpossible) are the outputs y₁ from the model. Each of the outputsgenerated by the linear (or linearised) model comprises a linearcombination of the states x, and inputs u_(i), and so the outputs can bedefined by the “output” or “measurement” equation:

Y(t)=CX(t)+DU(t)Mv(t)

Where C is a j×n matrix linking the outputs to the states,

D is a j×m matrix linking the outputs to the inputs,

j is the number of outputs, and

Mv(t) is a quantity (represented by an n×1 vector) called the“measurement noise”.

The measurement noise represents errors and noise that invariably existin measurements taken from the actual vehicle. Like Ew(t) above, Mv(t)is assumed to be Gaussian, white, have zero mean, to act directly on thestate derivatives and to be uncorrelated with the process noise oritself.

Next, it will be noted that both the state equation and the measurementequation defined above are continuous functions of time. However,continuous time functions do not often lend themselves to easy digitalimplementation (such as will generally be required in implementing thepresent invention) because digital control systems generally operate asrecursively repeating algorithms. Therefore, for the purpose ofimplementing the equations digitally, the continuous time equations maybe converted into the following recursive discrete time equations bymaking the substitutions set out below and noting that (according to theprinciple of superposition) the overall response of a linear system isthe sum of the free (unforced) response of that system and the responsesof that system due to forcing/driving inputs. The recursive discretetime equations are:

X _(k+1=) FX _(k+) GU _(k+1+) Lw _(k+1)

Y _(k+1=) ZX _(k+) JU _(k+1+) Nv _(k+1)

where k+1 is the time step occurring immediately after time step k,

Z=C, J=D and Nv is the discrete time analog of the continuous timemeasurement noise Mv(t).

F is a transition matrix which governs the free response of the system.F is given by:

F=e^(A)δ

GU.sub.k+1 is the forced response of the system, i.e. the system'sresponse due to the driving inputs. It is defined by the convolutionintegral as follows:

GU _(k+1=)∫₀ ^(Δt) e ^(A(Δt−τ)dt)

Similarly, the quantity Lw.sub.k+1 is the (forced) response of thesystem due to the random “error” inputs that make up the process noise.Hence, conceptually this quantity may be defined as:

Lw _(k+1=)∫₀ ^(Δt) e ^(A(Δt−τ)dt) E _(w)(t _(k+1)+τ)dt

However, as noted above, the quantity Ew(t) is not deterministic and sothe integral defining Lw.sub.k+1 cannot be performed (even numerically).It is for this reason that it is preferable to use statistical filteringtechniquτes such as a “Kalman Filter” to statistically optimise thestates estimated by the mathematical model.

In general, a “Kalman Filter” operates as a “predictor-corrector”algorithm. Hence, the algorithm operates by first using the mathematicalmodel to “predict” the value of each of the states at time step k+1based on the known inputs at time step k+1 and the known value of thestates from the previous time step k. It then “corrects” the predictedvalue using actual measurements taken from the vehicle at time step k+1and the optimised statistical properties of the model. In summary, theKalman Filter comprises the following equations each of which iscomputed in the following order for each time step:

$\left. {{\left. \begin{matrix}{{\underset{\_}{X}}_{{k + 1}k} = {{F{\underset{\_}{X}}_{kk}} + {G{\underset{\_}{U}}_{k + 1}}}} \\{P_{{k + 1}k} = {{{FP}_{kk}F^{T}} + Q}} \\{K_{k + 1} = {P_{{k + 1}k}{Z^{T}\left( {{{ZP}_{{k + 1}k}Z^{T}} + R} \right)}^{- 1}}} \\{{\underset{\_}{Y}}_{k + 1} = {{Z{\underset{\_}{X}}_{{k + 1}k}} + {J{\underset{\_}{U}}_{k + 1}}}} \\{{\underset{\_}{v}}_{k + 1} = {{\hat{\underset{\_}{Y}}}_{k + 1} - {\underset{\_}{Y}}_{k + 1}}}\end{matrix} \right\} \mspace{14mu} {predictor}}\begin{matrix}{{\underset{\_}{X}}_{{k + 1}{k + 1}} = {{\underset{\_}{X}}_{{k + 1}k} + {K_{k + 1}v_{k + 1}}}} \\{P_{{k + 1}{k + 1}} = {\left( {I - {K_{k + 1}Z}} \right)P_{{k + 1}k}}}\end{matrix}} \right\} \mspace{14mu} {corrector}$

where the notation k+1|k means the value of the quantity in question attime step k+1 given information from time step k. Similarly, k+1|k+1means the value of the quantity at time step k+1 given updatedinformation from time step k+1. [0135]P is the co-variance in thedifference between the estimated and actual value of X. [0136]Q is theco-variance in the process noise. [0137]K is the “Kalman gain” which isa matrix of computed coefficients used to optimally “correct” theinitial state estimate. [0138]R is the co-variance in the measurementnoise. [0139] is a vector containing measurement values taken from theactual vehicle.

is a quantity called the “innovation” which is the difference betweenthe measured values actually taken from the vehicle and values for thecorresponding quantities estimated by the model.

The operation of the discrete time state space equations outlined above,including the Kalman gain and the overall feedback closed loop controlstructure, are represented graphically in FIG. 7.

In relation to the spatial database, it is mentioned above that a widerange of methods are known for arranging data within databases. Onecommonly used technique is to provide a “hash table”. The hash tabletypically operates as a form of index allowing the computer (in thiscase the control system CPU) to “look up” a particular piece of data inthe database (i.e. to look up the location of that piece of data inmemory). In the context of the present invention, pieces of datapertaining to particular locations along the vehicle's path are assigneddifferent hash keys based on the spatial location to which they relate.The hash table then lists a corresponding memory location for each hashkey. Thus, the CPU is able to “look up” data pertaining to a particularlocation by looking up the hash key for that location in the hash tablewhich then gives the corresponding location for the particular piece ofdata in memory. In order to increase the speed with which these queriescan be carried out, the hash keys for different pieces of spatial datacan be assigned in such a way that “locality” is maintained. In otherwords, points which are close to each other in the real world should begiven closely related indices in the hash table (i.e. closely relatedhash keys).

The spatial hash algorithm used to generate hash keys for differentspatial locations in representative embodiments of the present inventionmay be most easily explained by way of a series of examples. To begin,it is useful to consider the hypothetical vehicle path trajectory shownin FIG. 8. In FIG. 8, the successive points which define the path aredescribed by a simplified integer based (X,Y) coordinate system. Hence,in FIG. 8, the vehicle moves in the X direction along the entire lengthof the first swath from (0,0) to (4,0), before moving up in the Ydirection to then move back along the second swath in the oppositedirection from (4,1) to (0,1), etc.

As outlined above, in the present invention all data is stored withinthe spatial database with reference to spatial location. Therefore, itis necessary to assign indices or “hash keys” to each piece of databased on the spatial location to which each said piece of data relates.However, it will be recalled that the hash table must operate by listingthe hash key for each particular spatial location together with thecorresponding memory location for data pertaining to that spatiallocation. Therefore, the hash table is inherently one-dimensional, andyet it must be used to link hash keys to corresponding memoryallocations for data that inherently pertains to two-dimensional space.

One simple way of overcoming this problem would be to simply assign hashkeys to each spatial location based only on, say, the Y coordinate ateach location. The hash keys generated in this way for each point on thevehicle path in FIG. 8 are given in Table 1 below.

TABLE 1 Spatial Hash Key Generated Using Only the Y Coordinate (X, Y)Hash key (X, Y) Hash key coor- (hexa- Hash key coor- (hexa- Hash keydinates decimal) (decimal) dinates decimal) (decimal) (0, 0) 0 × 0 0 (3,2) 0 × 2 2 (1, 0) 0 × 0 0 (4, 2) 0 × 2 2 (2, 0) 0 × 0 0 (0, 3) 0 × 3 3(3, 0) 0 × 0 0 (1, 3) 0 × 3 3 (4, 0) 0 × 0 0 (2, 3) 0 × 3 3 (0, 1) 0 × 11 (3, 3) 0 × 3 3 (1, 1) 0 × 1 1 (4, 3) 0 × 3 3 (2, 1) 0 × 1 1 (0, 4) 0 ×4 4 (3, 1) 0 × 1 1 (1, 4) 0 × 4 4 (4, 1) 0 × 1 1 (2, 4) 0 × 4 4 (0, 2) 0× 2 2 (3, 4) 0 × 4 4 (1, 2) 0 × 2 2 (4, 4) 0 × 4 4 (2, 2) 0 × 2 2

The prefix “0x” indicates that the numbers in question are expressed inhexadecimal format. This is a conventional notation.

Those skilled in the art will recognise that the above method forgenerating hash keys is far from optimal because there are five distinctspatial locations assigned to each different hash key. Furthermore, inmany instances, this method assigns the same hash key to spatiallocations which are physically remote from each other. For instance, thepoint (0,1) is distant from the point (4,1), and yet both locations areassigned the same hash key. An identically ineffective result would beobtained by generating a hash key based on only the X coordinate.

An alternative method would be to generate hash keys by concatenatingthe X and Y coordinates for each location. The hash keys generated usingthis method for each point on the vehicle path in FIG. 8 are given inTable 2 below.

TABLE 2 Hash Keys Generated by Concatenating the X and Y Coordinates (X,Y) Hash key (X, Y) Hash key coor- (hexa- Hash key coor- (hexa- Hash keydinates decimal) (decimal) dinates decimal) (decimal) (0, 0) 0 × 0  0(3, 2) 0 × 302 770 (1, 0) 0 × 100 256 (4, 2) 0 × 402 1026 (2, 0) 0 × 200512 (0, 3) 0 × 3  3

In order to understand how the numbers listed in Table 2 above werearrived at, it is necessary to recognise that in the digitalimplementation of the present control system, all coordinates will berepresented in binary. For the purposes of the present example whichrelates to the simplified integer based coordinate system in FIG. 8, asimplified 8-bit binary representation has been used.

Hence, to illustrate the operation of the spatial hash key algorithmused to generate the numbers in Table 2, consider the point (3,3). Thoseskilled in the art will understand that the decimal number 3 may bewritten as 11 in binary notation. Therefore, the location (3,3) may berewritten in 8-bit binary array notation as (00000011,00000011).Concatenating these binary coordinates then gives the single 16-bitbinary hash key 0000001100000011 which can equivalently be written asthe hexadecimal number 0x303 or the decimal number 771. The process ofconverting between decimal, binary and hexadecimal representationsshould be well known to those skilled in the art and need not beexplained.

It will be noted from Table 2 above that concatenating the X and Ycoordinates leads to unique hash keys (in this example) for each spatiallocation. However, the hash keys generated in this way are stillsomewhat sub-optimal because points which are located close to eachother are often assigned vastly differing hash keys. For example,consider the points (0,0) and (1,0). These are adjacent point in the“real world”. However, the hash keys assigned to these points using thismethod (written in decimal notation) are 0 and 256 respectively. Incontrast, the point (0,4) is much further away from (0,0) and yet it isassigned the much closer hash key 4. Therefore, this algorithm does notmaintain “locality”, and an alternative algorithm would be preferable.

Yet a further method for generating hash keys is to use a techniquewhich shall hereinafter be referred to as “bitwise interleaving”. As forthe previous example, the first step in this technique is to representthe (X,Y) coordinates in binary form. Hence, using the 8-bit binaryarray representation discussed above, the point (X,Y) may be re-writtenin 8-bit binary array notation as (X₁X₂X₃X₄X₅X₆X₇X₈, Y₁Y₂Y₃Y₄Y₅Y₆Y₇Y₈).Next, rather than concatenating the X and Y coordinates to arrive at asingle 16-bit binary hash key, the successive bits from the X and Ybinary coordinates are alternatingly “interleaved” to give the following16-bit binary hash key X₁Y₁X₂Y₂X₃Y₃X₄Y₄X₅Y₅X₆Y₆X₇YX₈Y₈. The hash keysgenerated using this method for each point on the vehicle path in FIG. 8are given in Table 3 below.

TABLE 3 Hash Keys Generated by “Bitwise Interleaving” the X and YCoordinates (X, Y) (X, Y) Hash key (X, Y) Hash key coor- (hexa- Hash keycoor- (hexa- Hash key dinates decimal) (decimal) dinates decimal)(decimal) (0, 0) 0 × 0 0 (3, 2) 0 × e 14 (1, 0) 0 × 2 2 (4, 2)  0 × 2436 (2, 0) 0 × 8 8 (0, 3) 0 × 5 5 (3, 0) 0 × a 10 (1, 3) 0 × 6 7 (4, 0) 0 × 20 32 (2, 3) 0 × d 13 (0, 1) 0 × 1 1 (3, 3) 0 × f 15 (1, 1) 0 × 3 3(4, 3)  0 × 25 37 (2, 1) 0 × 9 9 (0, 4)  0 × 10 16 (3, 1) 0 × b 11 (1,4)  0 × 12 18 (4, 1)  0 × 21 33 (2, 4)  0 × 18 24 (0, 2) 0 × 4 4 (3, 4) 0 × 1a 26 (1, 2) 0 × 6 6 (4, 4)  0 × 30 48 (2, 2) 0 × e 12

To further illustrate the operation of the spatial hash algorithm usedto generate the numbers in Table 3, consider the point (3,4). As notedabove, the decimal number 3 may be written as 11 in binary notation.Similarly, decimal number 4 is written as 100 in binary. Therefore, thelocation (3,4) may be rewritten in 8-bit binary array notation as(00000011,00000100). Bitwise interleaving these binary coordinates thengives the single 16-bit binary hash key 0000000000011010, which canequivalently be written as the hexadecimal number 0x1a or the decimalnumber 26.

From Table 3 it will be seen that generating hash keys by “bitwiseinterleaving” the X and Y coordinates leads to unique hash keys (in thisexample) for each spatial location. Also, the hash keys generated inthis way satisfy the requirement that points which are close together inthe real world are assigned closely related hash keys. For example,consider again the points (0,0) and (1,0). The hash keys now assigned tothese points by “bitwise interleaving” (when written in decimalnotation) are 0 and 2 respectively. Furthermore, the point (0,1) whichis also nearby is also assigned the closely related hash key 1.Conversely, points which are separated by a considerable distance in thereal world are given considerably differing hash keys, for example, thehash key for (4,3) is 37.

From the example described with reference to Table 3, it can be seenthat generating hash keys by “bitwise interleaving” the binary X and Ycoordinates preserves “locality”. This example therefore conceptuallyillustrates the operation of the bitwise interleaving spatial hashalgorithm that may be used with representative embodiments of thepresent invention. However, the above example is based on the simplifiedinteger based coordinate system shown in FIG. 8. In order to understandthe actual algorithm that may be used in the implementation of thepresent control system, it is necessary to take into account certainother complexities. These complexities include:

The fact that GPS and other similar systems which describe spatiallocation typically do so using IEEE double-precision floating-pointnumbers (not simple integers). For instance, GPS supplies coordinates inthe form of (X,Y) coordinates where X corresponds to longitude, and Ycorresponds to latitude. Both X and Y are given in units of decimaldegrees.

the fact that certain spatial locations have negative coordinate valueswhen described using GPS and other similar coordinate systems. Forexample, using the WGS84 datum used by current GPS, the coordinates(153.00341,−27.47988) correspond to a location in Queensland, Australia(the negative latitude value indicates southern hemisphere).

Complexities inherent in representing numbers in accordance with theIEEE double-precision floating-point numbers standard.

FIG. 9 shows an example vehicle path similar to that shown in FIG. 8,except that the coordinates used to describe the points along the pathin FIG. 9 correspond to a “realistic” coordinate system such as thatused by current GPS. In order to understand the implementation of thebitwise interleaving spatial hash algorithm when applied to theserealistic coordinates, it is necessary to first appreciate certainaspects regarding the way numbers are represented using the standardIEEE double-precision floating-point number format.

A double-precision floating-point number represented in accordance withthe IEEE 754 standard comprises a string of 64 binary characters (64bits) as shown in FIG. 10. The number is represented in three parts,namely the sign, the exponent and the mantissa. The sign comprises onebit. If the sign bit is 1 then the number is negative, and conversely ifthe sign bit is 0 then the number is positive. The exponent compriseseleven binary characters, and hence can range from 00000000000 to11111111111. However, because of the need to represent numbers that areboth greater and smaller than one, it is necessary to be able torepresent both large positive and large negative values for theexponent. However, it is not desirable to use one of the exponent bitsto represent the sign of the exponent because this would leave fewerbits available to represent the exponent's actual value and wouldtherefore greatly limit the size of the numbers that could berepresented. Therefore, in the IEEE standard 64 bit format, the truevalue of the exponent is given by the binary number actually written bythe eleven exponent bits minus an implied exponent bias.

Hence, Actual exponent value=written exponent value−exponent bais

The exponent bias is 0x3ff=1023. Consequently, the maximum true exponentvalue that can be represented (written in decimal notation) is 1023, andthe minimum true exponent value that can be represented is −1022.

Finally, the remaining 52 bits form the mantissa. However, as allnon-zero numbers must necessarily have a leading “1” when written inbinary notation, an implicit “1” followed by a binary point is assumedto exist at the front of the mantissa. In other words, the leading “1”and the binary point which must necessarily exist for all non-zerobinary numbers is simply omitted from the actual written mantissa in theIEEE 64-bit standard format. This is so that an additional bit may beused to represent the number with greater precision. However, wheninterpreting numbers which are represented in accordance with the IEEEstandard, it is important to remember that this leading “1” and thebinary point implicitly exist even though they are not written.

Bearing in mind these issues, it is possible to understand the actualspatial hash algorithm used in representative implementations of thepresent control system. A “worked” example illustrating the operation ofthe spatial hash algorithm to generate a hash key based on thecoordinate (153.0000°, −27.0000° is given in the form of a flow diagramin FIG. 11. The points are initially expressed in terms of decimaldegrees as this is the format in which they are delivered from, forexample, GPS.

From FIG. 11 it can be seen that in order to implement the algorithm theX and Y coordinates are separated. The next step is to “normalise” thesigns of the respective coordinates (in this case only the Y coordinateneeds to be normalized). The reason for normalising the signs of thecoordinate is because, when calculating a spatial hash key, it is moreconvenient to eliminate negative sign bits from the coordinates. In thecase of the latitude coordinate, those skilled in this area willrecognise that latitude is conventionally written as a number in therange (−90°≦latitude≦90°). Therefore, by simply adding 90° to the valueof the latitude coordinate, the spatial hash algorithm can operate withvalues in the equivalent “un-signed” or “normalised” latitude range(0°≦latitude≦180°). Those skilled in the art will appreciate that thelongitude coordinates can also be normalised to fall within the range(0° longitude≦360.degree.), although that is not necessary in thisexample.

After normalising the coordinates, the next step is to convert therespective coordinates from their representations in decimal degreesinto binary IEEE double-precision floating-point number format. This isshown as step 3) in FIG. 11. However, it will be noted that the binarycoordinate representations (and all other numbers which are generated orused by the algorithm in binary form) have been written in thealternative hexadecimal notation for ease of reference and to save spacein FIG. 11.

Next, the binary representations of the two coordinates are split intotheir respective exponent (11 bits) and mantissa (52 bits) portions.This is step 4) in FIG. 11. Then, in order to determine the correct(“true”) value of the exponent, the exponent for each of the coordinateis “de-biased” by subtracting the implicit exponent bias (0x3ff=1023) asdescribed above. This is step 5).

After de-biasing the exponents, the resulting exponents are thenadjusted by a selected offset. The size of the offset is selecteddepending on the desired “granularity” of the resulting fix-pointnumber. In the particular example shown in step 6) of FIG. 11, theoffset is 37, however those skilled in the art will appreciate thisnumber can be varied to suit.

After adjusting the exponent, the next step is to “resurrect” theleading “1” and the binary point which implicitly exist in the mantissabut which are left off when the mantissa is actually written (seeabove). Hence, the leading “1” and the binary point are simply prependedto the mantissa of each of the coordinates. This is step 7) in FIG. 11.

The mantissa for each coordinate is then right-shifted by the number ofbits in the corresponding exponent. The exponents for each coordinateare then prepended to their corresponding mantissas forming a singlecharacter string for each coordinate. There is then an optional step ofdiscarding the high-order byte for each of the two bit fields. This maybe done simply to save memory if required, but is not necessary.Finally, the resultant bit fields for each coordinate are bitwiseinterleaved to obtain a single hash key corresponding to the originalcoordinates. In the example shown in FIG. 11, the resultant hash key is32-bits in length. However, the length of the resultant hash key mayvary depending on, for example whether the high-order byte is discarded,etc.

Those skilled in the art will recognise that various other alterationsand modifications may be made to the particular embodiments, aspects andfeatures of the invention described without departing from the spiritand scope of the invention.

1. A control system as claimed in claim 1, wherein data is arrangedwithin the database in accordance with a hash table which relates thememory allocations for the different items of spatial data within thedatabase to corresponding indices.
 2. A control system as claimed inclaim 30, wherein the indices for the different items of data aredetermined according to the spatial location to which the respectiveitems of data pertain.
 3. A control system as claimed in claim 31,wherein a spatial hash key algorithm is used to generate the indices. 4.A control system as claimed in claim 32, wherein the spatial hash keyalgorithm operates so that data pertaining to spatial locations whichare close to each other receive closely related indices.
 5. A controlsystem as claimed in claim 33, wherein the spatial hash key algorithmuses bitwise interleaving.
 6. A control system as claimed in claim 34,wherein the spatial hash key algorithm uses double-precisionfloating-point numbers.
 7. A control system as claimed in claim 1,wherein the control system is adapted to receive data from at least oneexternal source.
 8. A control system as claimed in claim 36, wherein thecontrol system is adapted to receive data from at least one externalsource at control speed.
 9. A control system as claimed in claim 36,wherein the data received from the at least one external source is usedto generate estimates of the vehicle's pose.
 10. A control system asclaimed in claim 36, wherein the at least one external data sourceincludes GPS.
 11. A control system as claimed in claim 39 wherein theGPS is supplemented by a SBAS.
 12. A control system as claimed in claim36, wherein the at least one external data source includes an INS.
 13. Acontrol system as claimed in claim 41 wherein the INS includes one ormore rate gyros, accelerometers or a combination of both.
 14. A controlsystem as claimed in claim 39, wherein the GPS is supplemented by aGBAS.
 15. A control system as claimed in claim 36, wherein the at leastone external data source includes machine vision and/or image analysis.16. A control system as claimed in claim 36, wherein the at least oneexternal data source includes LIDAR.
 17. A control system as claimed inclaim 36, wherein the at least one external data source includes amagnetometer.
 18. A control system as claimed in claim 36, wherein theat least one external data source includes a tilt sensor.
 19. A controlsystem as claimed in claim 36, wherein the at least one external datasource includes ultrasonic range and direction finding.
 20. A controlsystem as claimed in claim 36, wherein a filter is used to obtain astatistically optimised estimate of the state of the vehicle.
 21. Acontrol system as claimed in claim 48, wherein the filter utilises thedata from the one or more external data sources to obtain the optimisedestimate.
 22. A control system as claimed in claim 49, wherein thefilter is a Kalman filter.
 23. A control system as claimed in claim 1,including means for interpreting a user-defined path and converting thesaid user-defined path into a plurality of points representing theuser-defined path.
 24. A control system as claimed in claim 51,including means for generating a desired position, heading andinstantaneous radius of curvature for the vehicle at each point on theuser-defined path.
 25. A control system as claimed in claim 52, whereinthe points representing the user-defined path and the desired position,heading and instantaneous radius of curvature for the vehicle areentered into the spatial database.
 26. A control system as claimed inclaim 53, including means for calculating an error term relating to thedifference between the vehicle's actual position, heading andinstantaneous radius of curvature and the vehicle's desired position,heading and instantaneous radius of curvature.
 27. A control system asclaimed in claim 54, wherein the controller uses the error term togenerate a control signal for controlling the vehicle.
 28. A method forcontrolling a vehicle comprising entering spatial data relating to aregion to be traversed by the vehicle into a spatial database, providingspatial data from the spatial database to a controller at control speedto control the vehicle as the vehicle traverses the region, and enteringupdated spatial data into the spatial database as the vehicle traversesthe region.
 29. A method for controlling spatial data from the spatialdatabase to a controller at control speed to control the vehicle as thevehicle traverses the region, and updated spatial data being receivedinto the spatial database as the vehicle traverses the region.